#include"visual_odom/show_pub.h"

namespace test_visual_odom{

void ShowPub::pub_frame(shared_ptr<Frame> p_frame){
    if(!p_last_frame_){
        img_height_ = p_frame->left_img_.size().height;
        img_width_ = p_frame->left_img_.size().width;
        p_last_frame_ = p_frame;
        return;
    }
    p_current_frame_ = p_frame;
    cv::Mat img_show(img_height_,2*img_width_, CV_8UC3);
    cv::hconcat(plotFrameImage(p_last_frame_), plotFrameImage(p_current_frame_), img_show);
    sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", img_show).toImageMsg();  //opencv 是按照bgr8显示的,不正确的话,图像显示有问题
    frame_pub_.publish(msg);
    p_last_frame_ = p_current_frame_;
}

cv::Mat ShowPub::plotFrameImage(shared_ptr<Frame> p_frame) {
    cv::Mat img_out(p_frame->left_img_);
    for (size_t i = 0; i < p_frame->feature_points_left_.size(); ++i) {
        auto feat = p_frame->feature_points_left_[i];
        if (p_frame->feature_points_left_[i]->p_landmark_.lock()) {
            cv::circle(img_out, feat->keyPoint2D_.pt, 2, cv::Scalar(0, 250, 0),
                       2);
        }else{
            cv::circle(img_out, feat->keyPoint2D_.pt, 2, cv::Scalar(0, 0, 250),
                       2);
        }
    }
    return img_out;
}

void ShowPub::pub_landmark(){
    sensor_msgs::PointCloud2 pc_plane_display;
    pcl::toROSMsg(*pc_landmarks_, pc_plane_display);
    pc_plane_display.header.frame_id = "nexus0/velodyne";
    pc_landmarks_pub_.publish(pc_plane_display);
}

void ShowPub::updateMap(shared_ptr<Map> map){
    all_landmarks_ = map->getAllLandmarks();
    pc_landmarks_.reset(new pcl::PointCloud<PointType>());
    for(auto lm:all_landmarks_){
        pcl::PointXYZI p;
        p.x = lm.second->pos_world_[0];
        p.y = lm.second->pos_world_[1];
        p.z = lm.second->pos_world_[2];
        pc_landmarks_->points.emplace_back(p);
    }
}

}